import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory

#去找到 这个 urdf 目录，并且拼接一个完整的目录
def generate_launch_description():
    #获取默认的urdf文件路径,get_package_share_directory("fishbot_description") 这里获取到的是 install/fishbot_description/share/ 这个目录。
    urdf_file = get_package_share_directory("fishbot_description")
    #拼接完整的urdf文件路径
    urdf_path = urdf_file + "/urdf/first_robot.urdf"
    default_rviz_config_path = urdf_file + "/config/display_robot_model.rviz"
    
    #声明一个urdf目录参数，方便修改(就是外部传参数过来的)
    action_declare_arg_model_path = launch.actions.DeclareLaunchArgument(
        name='model',
        default_value=str(urdf_path),
        description='加载的模型文件的绝对路径'
    )

    #通过文件路径，通过cat获取内容，并转换成参数值对象，以供传入 robot_state_publisher
    substitutions_command_result = launch.substitutions.Command(
        #注意 这里cat 要加空格，实际commad 打印在控制台是这样的。cat /home/caixaing/ros2/CHAPT6/install/fishbot_description/share/fishbot_description/urdf/first_robot.urdf
        ['xacro ',launch.substitutions.LaunchConfiguration('model')]
    )
    robot_description = launch_ros.parameter_descriptions.ParameterValue(
        substitutions_command_result,
        value_type=str
    )

    #状态发布节点
    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        #这样写 相当于在命令行执行：ros2 run robot_state_publisher robot_state_publisher --robot_description "$(cat /home/caixaing/ros2/CHAPT6/install/fishbot_description/share/fishbot_description/urdf/first_robot.urdf)"
        parameters=[{'robot_description':robot_description}]
    )

    #关节状态发布节点
    joint_state_publisher_node = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher'
    )

    #rviz2节点
    rviz2_node = launch_ros.actions.Node(
        package='rviz2',
        executable='rviz2',
        #这样写 相当于在命令行执行：ros2 run rviz2 rviz2 -d /home/caixaing/ros2/CHAPT6/install/fishbot_description/share/fishbot_description/config/display_robot_model.rviz
        arguments=['-d',default_rviz_config_path]
    )

    #启动所有节点
    return launch.LaunchDescription([
        action_declare_arg_model_path,
        joint_state_publisher_node,
        robot_state_publisher_node,
        rviz2_node
    ])